#include "servomotor.h"
#include <QDebug>

Servomotor::Servomotor(AbstractSystem *a_sys, int a_id, int freq, double a_delay, double a_tau, double a_imprecision):
    AbstractMotor(1000/freq),sys(a_sys), id(a_id), tau(a_tau), imprecision(a_imprecision)
{
    currentCons = 0;
    dt = 1./freq;
    delay = static_cast<int>(a_delay*freq);

    timer->start();
}

void Servomotor::update()
{
   for(int i=0; i<consigns.size(); i++)
        consigns[i].countdown--;
   if(consigns.size() > 0 && consigns[0].countdown < 0)
   {
       currentCons = consigns[0].cons;
       consigns.pop_front();
   }

    if(tau == 0)
        state = currentCons;
    else
        state += ( dt*(currentCons - state) ) / tau;

    //TODO gaussian noise on result

    sys->checkValue(id);
}

void Servomotor::setConsign(double cons)
{
    consigns << Consign(360*cons, delay);
}

double Servomotor::getState()
{
    return(state);
}

void Servomotor::setState(double s)
{
    state = s;
}
